classdef PlatformLogic < Logic
    
  methods
    % CONSTRUCTOR
    function O = PlatformLogic(varargin)
      global CG;          
      O = setName(O,'Platform');
      
      ModuleTypes = {'NI','Arduino'};
      ModuleNames = {'NIDAQ','ArduinoPlatform'};
      ModulePars = {{'ChAI',[],'ChAO',[],'ChDO',[],'SRAI',10000,'SRAO',10000,'Name',ModuleNames{1}},[]};
      IDs = addModules(O,ModuleTypes,ModuleNames,ModulePars);
      O.HW.IDs.DAQ = IDs(1);
      O.HW.IDs.ArduinoPlat = IDs(2);
       
      % HW RELATED
      O.HW.Platforms = CG.Parameters.Setup.Platforms;
      O.HW.NPlatforms = length(O.HW.Platforms);
      O.HW.Triggerline = 2;
      O.HW.NAnimalSensors = length([O.HW.Platforms.AnimalSensors]);
      % ATTACH SENSOR CHANNELS TO NIDAQ
      
      % SENSOR CHANNELS OF THE PLATFORM
      for iP=1:O.HW.NPlatforms
        cP = CG.Parameters.Setup.Platforms(iP);
        for iS=1:length(O.HW.Platforms(iP).AnimalSensors)
          O.setInputChannel(cP.Device{1},CG.Parameters.Setup.DAQID,...
            cP.AnimalSensors{iS},cP.AnimalSensorThresholds(iS),...
            ['AniPosP',num2str(iP),'S',num2str(iS)]);
        end
        O.setInputChannel(cP.Device{1},CG.Parameters.Setup.DAQID ,...
          cP.PositionSensors{1},cP.PositionSensorThresholds(1),...
          ['PlatPosP',num2str(iP)]);
      end
      
      PhysNames = {'AniPosP1S1','AniPosP1S2','AniPosP1S3','AniPosP2S3','AniPosP2S2','AniPosP2S1'};
      for i=1:length(PhysNames)
        O.HW.AnimalSensorsPhys(i) = getInputByName(O,PhysNames{i});
      end
      
      % TRIAL AND CAMERA TRIGGER
      O.setInputChannel('NI',CG.Parameters.Setup.DAQID,'ai8',2.5,['Trial']);
      O.setDOutputChannel('NI',CG.Parameters.Setup.DAQID,CG.Parameters.Setup.DAQ.TrialChannel,'Trial');
      C_setDigitalNI(O.HW.IDs.DAQ,'Trial',0);
      
    end
    
    % INITIALIZE THE PLATFORMS (MOVE TO CENTER POSITION)
    function initializePlatforms(O)
      global CG
      for iP=1:O.HW.NPlatforms
        WaitTime = O.HW.Platforms(iP).WaitTime;
        StepTime = O.HW.Platforms(iP).StepTime;
        InterStepTime = StepTime;
        cInput = O.getInputByName(['PlatPosP',num2str(iP)]);
        cStepSize = CG.Parameters.Setup.Platforms(iP).StepSize;
        InputCovered = ~O.States.InputStates(cInput);
        % QUICKLY Move Platform Inward
        while ~InputCovered 
          O.movePlatform(iP,-cStepSize);
          pause(WaitTime); %while CG.Sessions.Arduino(O.HW.IDs.ArduinoPlat).BusyState pause(InterStepTime); end
          O.updateStates;
          InputCovered = ~O.States.InputStates(cInput);
        end
        % QUICKLY Move Platform Outward
        while InputCovered 
          O.movePlatform(iP,cStepSize); 
          pause(WaitTime); %while  CG.Sessions.Arduino(O.HW.IDs.ArduinoPlat).BusyState pause(InterStepTime); end
          O.updateStates;
          InputCovered = ~O.States.InputStates(cInput);
        end
        % FINE ADJUSTMENT UNTIL Input JUST COVERED
        while ~InputCovered % SLOWLY Move Platform Inward
          O.movePlatform(iP,-cStepSize/3); 
          pause(WaitTime/2); %while  CG.Sessions.Arduino(O.HW.IDs.ArduinoPlat).BusyState pause(InterStepTime); end
          O.updateStates;
          InputCovered = ~O.States.InputStates(cInput);          
        end
        O.movePlatform(iP,-cStepSize); 
        O.HW.Platforms(iP).Position = 0;
      end
    end
    
    % MOVE A PLATFORM
    function movePlatform(O,PID,Distance)
      MotorID = O.HW.Platforms(PID).MotorID;
      DistanceSteps = -Distance * O.HW.Platforms(PID).MotorMM2Step;
      C_sendMessageArduino(O.HW.IDs.ArduinoPlat,...
        'movemotor', {MotorID, sign(DistanceSteps), abs(DistanceSteps)});
      O.HW.Platforms(PID).Position = O.HW.Platforms(PID).Position + Distance;
    end
      
    % MOVE THE GATES (OPEN, CLOSE, ARBITRARY ANGLE)
    function moveGate(O,GateID,State)
      global CG;
      switch State
        case 'open'; Angle = O.HW.Platforms(GateID).GateOpenAngle;
        case 'closed'; Angle = O.HW.Platforms(GateID).GateCloseAngle;
        otherwise
          if isnumeric(State)
            Angle = State;
          else
            fprintf(['Command ''',State,''' not recognized.\n']); return;
          end
      end
      C_sendMessageArduino(O.HW.IDs.ArduinoPlat,'moveservo', ...
        {GateID, Angle, CG.Parameters.Setup.Platforms(GateID).GateVelocity});
      O.HW.Platforms(GateID).GateState = State;
    end    
    
    function moveGates(O,GateIDs,State)
      % OPEN GATES
      switch State
        case 'open'; CheckState = 'closed';
        case 'closed'; CheckState = 'open';
      end
      for iP=1:length(GateIDs)
        cGateID = GateIDs(iP);
        if strcmp(O.HW.Platforms(cGateID).GateState,CheckState);
          O.moveGate(cGateID,State); 
          if iP<length(GateIDs) pause(2);  end
        end
      end
    end
          
  end
end